// system include.
#include <pthread.h>
#include <thread>
#include <iostream>
#include <fstream>
#include <string.h>
#include <unistd.h> // 引入 usleep()
// apply include.
#include <mutex>

#include "control.pb.h"
#include "localization.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>


int main(int argc, char *argv[])
{
    float speed = 0;
    eCAL::Initialize(argc, argv, "GNGGA Publisher");
    eCAL::protobuf::CPublisher<MowerProtoPakg::localizationGNGGA> ecalGNGGApublisher("GNGGAMessage");
    eCAL::protobuf::CPublisher<MowerProtoPakg::controlMoveOrder> ecalcontrolMovepublisher("ControlMessage");
    while (eCAL::Ok()) {
        // 测试定位订阅数据
        // 把数据包装成protobuf
        MowerProtoPakg::localizationGNGGA localizationGNGGAMessage;// 创建一个protobuf消息对象
        localizationGNGGAMessage.set_time(010337.10);
        localizationGNGGAMessage.set_latitude(3040.25368);
        localizationGNGGAMessage.set_latituderange("N");
        localizationGNGGAMessage.set_longitude(10406.42449);
        localizationGNGGAMessage.set_longituderange("E");
        localizationGNGGAMessage.set_gpsstatus(4);
        localizationGNGGAMessage.set_startnum(12);
        localizationGNGGAMessage.set_precision(1);
        localizationGNGGAMessage.set_altitude(18);
        localizationGNGGAMessage.set_geoidheight(587.7);
        // Send the message
        ecalGNGGApublisher.Send(localizationGNGGAMessage);
        std::cout << "Sent message!" << std::endl << std::endl;

        // 测试控制订阅数据
        MowerProtoPakg::controlMoveOrder controlMessage;// 创建一个protobuf消息对象 发布控制指令
        speed+=0.04;
        if(speed>0.5)speed = 0;
        controlMessage.set_linespeed(0);
        controlMessage.set_angularspeed(0.1);
        ecalcontrolMovepublisher.Send(controlMessage);
        printf("pub!\r\n");
        usleep(50000);
        //std::this_thread::sleep_for(std::chrono::milliseconds());
    }
    // finalize eCAL API
    eCAL::Finalize();
}

